% opt_script_single_step_equilibrium

clc
clear all
close all
format compact

base_filename = 'onehop_';
subdirectory_name = ['Data'];% filesep 'onestanceequil'];

time_stamp = clock;
time_stamp_suffix = [num2str(time_stamp(1)), '_', num2str(time_stamp(2)), '_', ...
    num2str(time_stamp(3)), '_', num2str(time_stamp(4)), '_', ...
    num2str(time_stamp(5)), '_', num2str(floor(time_stamp(6)))];
filename = [subdirectory_name filesep base_filename time_stamp_suffix];


MAX_STANCE_TIME = 0.35 ; % 0.5
NUM_DISCRETE_TORQUES = 8; % 8 % 15 % 26

x_dot = 2.5;
y = 1.0;

uniform_damping = 2.5e3;

E_net_opt = 10;
E_net_fc = 0;

% while(abs(E_net_opt) > 3*abs(E_net_fc))
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    initial_constant_torque = rand*10;  %2.8; %0.100;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    initial_torque_vector = 10*rand(NUM_DISCRETE_TORQUES,1); %initial_constant_torque*ones(NUM_DISCRETE_TORQUES,1);
    
    first_guess = (5*pi/8);
    
    pause(0.5)
    
    td_angle = fc_fixed_point_solve;
    
%     tic
%     [torque_vector, output_opt, J_opt, J_fsolve, torque_fsolve] = fc_opt_stance;
%     toc
    
%     [time_opt, force_opt] = get_force_profile(torque_vector);
    
    [time_passive, force_passive] = passive_force_profile;
    
    [time_fc, force_fc, output_fc, J_fc, controller_fc] = run_force_control(time_passive, force_passive);
    
    Kp = controller_fc.Kp;
    Ki = controller_fc.Ki;
    LIMITS_ON = controller_fc.LIMITS_ON;
    torque_limit = controller_fc.torque_limit;
    
%     J_opt
    J_fc
    
    close all
    plot(time_passive, force_passive, 'k--', time_fc, force_fc, 'b-')
    
    % close all
    % plot(time_passive, force_passive, 'k--', time_fc, force_fc, 'b-')
    
    
%     disp('Optimization Fidelity')
%     initial_constant_torque
%     dxout_opt = output_opt{3,2}(end,ForceControlModelClass.dxci)
%     yout_opt = output_opt{3,2}(end,ForceControlModelClass.yci)
%     disp(' ')
%     
%     dxout_opt_error = output_opt{3,2}(end,ForceControlModelClass.dxci) - x_dot
%     yout_opt_error = output_opt{3,2}(end,ForceControlModelClass.yci) - y
    
    dxout_fc = output_fc{3,2}(end,ForceControlModelClass.dxci);
    yout_fc = output_fc{3,2}(end,ForceControlModelClass.yci);
    
    dxout_fc_error = output_fc{3,2}(end,ForceControlModelClass.dxci) - x_dot
    yout_fc_error = output_fc{3,2}(end,ForceControlModelClass.yci) - y
    
    max_torque = controller_fc.max_torque
    
    T_in = 0.5*ForceControlModelClass.m*x_dot^2
    V_in = ForceControlModelClass.m*ForceControlModelClass.g*y
    
    T_out_fc = 0.5*ForceControlModelClass.m*dxout_fc^2
    V_out_fc = ForceControlModelClass.m*ForceControlModelClass.g*yout_fc
    
%     T_out_opt = 0.5*ForceControlModelClass.m*dxout_opt^2
%     V_out_opt = ForceControlModelClass.m*ForceControlModelClass.g*yout_opt
    
%     E_net_opt = T_out_opt + V_out_opt - T_in - V_in
    E_net_fc = T_out_fc + V_out_fc - T_in - V_in
    
% end

save(filename);

disp('DONE !!!')
disp('DONE !!!')
disp('DONE !!!')

initial_torque_vector